#include "drv_gm6020.h"

/**
 * @brief:    初始化
 */
void GM6020_Init(motor_gm6020_t* gm_ptr, uint16_t canId)
{
  gm_ptr->can_id         = canId;
  gm_ptr->in_id          = canId - 0x204;
  gm_ptr->rotor_angle    = 0;
  gm_ptr->rotor_speed    = 0;
  gm_ptr->torque_current = 0;
  gm_ptr->temp           = 0;
}

/**
 * @brief:    解析 GM6020 反馈帧数据包
 */
void GM6020_UpdateMotor(motor_gm6020_t* gm_ptr, uint8_t* arrData)
{
  gm_ptr->rotor_angle     = ((uint16_t)arrData[0] << 8) | arrData[1];
  gm_ptr->rotor_speed     = ((uint16_t)arrData[2] << 8) | arrData[3];
  gm_ptr->torque_current  = ((uint16_t)arrData[4] << 8) | arrData[5];
  gm_ptr->temp            = (uint16_t)arrData[6];
}

/**
 * @brief:    根据目标值，计算一次GM的输出电流
 */
algo_pid_t pid_spd_to_vol, pid_ang_to_spd;
float GM6020_CalcOutput(motor_gm6020_t* gm_ptr, uint16_t target_angle)
{ 
  float output_ang_to_spd = Algo_PID_Calc(&pid_ang_to_spd, gm_ptr->rotor_angle, target_angle);
  float output_spd_to_vol = Algo_PID_Calc(&pid_spd_to_vol, gm_ptr->rotor_speed, output_ang_to_spd);
  
  return output_spd_to_vol;
}




/**
 * @brief:    PWM 控制电机位置
 * @details:  angle 取 0-360, 角度制
 */
#define GM6020_THETA_MAX        360
void GM6020_DrivePosMode(motor_gm6020_t* gm_ptr, float angle)
{
  while (angle >= 360.0f) {
    angle -= 360.0f;
  }
  while (angle < 0.0f) {
    angle += 360.0f;
  }
  uint16_t plase = (unsigned int)((840.0f / GM6020_THETA_MAX) * angle + 1080.0f);
  BSP_PWM_SetPalse(plase);
}


uint8_t GM6020_GotoZero(void)
{
  BSP_PWM_Init();
  uint16_t delay = 0;
  
  while(1) {
    BSP_PWM_SetPalse(1020);
    delay++;
    HAL_Delay(40);
    if (delay > 40) {break; }
  }
  
  BSP_PWM_DeInit();
  return 1;
}
